package states;
import monitor.StateVariableMonitor;
import gui.OpCom;
import regulators.BallAndBeamRegul;
import regulators.BeamRegul;
import se.lth.control.realtime.DigitalIn;
import se.lth.control.realtime.DigitalOut;
public class PlaceBall extends StateClass implements State {

	int counter;
	double ref1 ;
	public PlaceBall(DigitalIn initialPosSensor, DigitalOut digitalSolenoid, BeamRegul beam,
			BallAndBeamRegul ballAndBeam, OpCom op, StateVariableMonitor stateVariableMonitor) {
		super(initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, stateVariableMonitor);


	}

	@Override
	public void excecute() {
		ref1+=0.12;
		beam.setRef(ref1);// 2.07
	}

	@Override
	public boolean stateOfExcecution() {
		if( !beam.controllerStatMonitor.hasReached(4.3)){
			return true;
		}
		return false;
	}

	@Override
	public void initialExcecution(){
		op.displayStep("PlaceBall");
		counter = 0;
		double ref = 0;
		ballAndBeam.setRef(ref);
		while( counter < 400){
			double ballPos = ballAndBeam.controllerStatMonitor.getBallposition1();
			if( ballPos <= ref +0.04 && ballPos >= ref -0.04){ // funkar +-0.2 50
				counter++;
			}
		}
		counter = 0;
		ref = 9;
		ballAndBeam.setRef(ref);
		while( counter < 400){
			double ballPos = ballAndBeam.controllerStatMonitor.getBallposition1();
			if( ballPos <= ref +0.04 && ballPos >= ref -0.04){ // funkar +-0.2 50
				counter++;
			}
		}
		ballAndBeam.shutDown();
		ref1 = ballAndBeam.controllerStatMonitor.getBeamAngle1();
		if(!beam.isRunning()) {
			beam.restart();
		}

	}

	@Override
	public void nextState() {
		beam.setRef(-8);
		while(!beam.controllerStatMonitor.hasReached(-10.0)){
		}
		beam.shutDown();
		setCurrentState(new ResetState(initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, stateVariableMonitor));

	}


	@Override
	public void suspend() {
		beam.shutDown();

	}

	@Override
	public void resume() {
		beam.restart();
		this.initialExcecution();

	}
}